close;
clear;
clc;

dual_zu5_model;
% iviz.ShowMarker = true;
jaka.DataFormat = 'column';
% [0 45 -90 180 180 0 180 135 90 0 180 0]
% ==================== 运动控制起点 ====================
startConfig = deg2rad([0 45 -90 180 180 0 180 135 90 0 180 0]');
% **************************************************

% ==================== 运动控制终点 ====================
% 无碰撞
% goalConfig = [-0.0265866960230835 1.20060928158556 -0.886613337506568 ...
%                 -0.224118765739356 -0.0209245320280914 -0.0925361298890841 ...
%                 0.949551091069362 0.755461596643079 -1.67978300858195 ...
%                 0.927575286511096 0.948158032013464 -0.00119973081986972 ...
%             ]';

% 有碰撞
goalConfig = [0.864609503852502 0.910498385798894 -1.86182871333987 ...
                0.965473021004514 0.868593188282488 -0.00609732484835337 ...
                -1.32278785260246 0.863670479869766 -1.98255496361084 ...
                1.12071779540657 -1.33021155415568 -0.000585607892029058
            ]';
% **************************************************

% ==================== 生成运动轨迹 ====================
[q,qd,qdd,tSamples,pp] = trapveltraj([startConfig goalConfig],300,'EndTime',3);

% ==================== 运动过程显示 ====================
r = rateControl(10);
hold on;
for i = 1:length(q)
    iviz.Configuration = q(:,i);
%     右手轨迹
    iviz.MarkerBodyName = "r_wrist_3_link";
    plot3(iviz.MarkerBodyPose(13), iviz.MarkerBodyPose(14), iviz.MarkerBodyPose(15),'r.');

%     左手轨迹
    iviz.MarkerBodyName = "l_wrist_3_link";
    plot3(iviz.MarkerBodyPose(13), iviz.MarkerBodyPose(14), iviz.MarkerBodyPose(15),'g.');

    iviz.ShowMarker = false;

    waitfor(r);
end




